﻿using System.Linq;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Schuifpuzzel.MyKinect;

namespace Schuifpuzzel
{
    /// <summary>
    /// Interaction logic for KinectColorView.xaml
    /// </summary>
    public partial class KinectColorViewer
    {
        public static BitmapSource LastDetectedImage;
        private Image _kinectColorImage;
        private ColorImageFormat _lastImageFormat = ColorImageFormat.Undefined;
        private byte[] _pixelData;
        private WriteableBitmap _outputImage;
        private static readonly int Bgr32BytesPerPixel = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;
        private Border border;

        /// <summary>
        /// Initialize the colorviewer, set size and rescale because kinect color stream is 640x480.
        /// </summary>
        public KinectColorViewer()
        {
            InitializeComponent();
            _kinectColorImage = new Image
                                    {
                                        RenderTransformOrigin = new Point(.5, .5),
                                        Stretch = Stretch.Uniform,
                                        Height = 240,
                                        Width = 320
                                    };
            KinectForPuzzle.GetInstance().SkeletonAndImageFramesReady += SkeletonAndImageFramesReady;
            KinectForPuzzle.GetInstance().KinectStatusChanged += KinectColorViewer_KinectStatusChanged;
            txtStatusMessage.Text = "No Kinect plugged in!";
        }

        /// <summary>
        /// Whenever the status changes, update the text to inform users.
        /// </summary>
        /// <param name="newKinectStatus">New status of kinect</param>
        void KinectColorViewer_KinectStatusChanged(KinectStatus newKinectStatus)
        {
            ImageContainer.Children.Clear();
            switch (newKinectStatus)
            {
                case KinectStatus.Connected:
                    txtStatusMessage.Text = "";
                    break;
                case KinectStatus.Disconnected:
                    txtStatusMessage.Text = "No Kinect plugged in!";
                    break;
                case KinectStatus.NotPowered:
                    txtStatusMessage.Text = "Plug in the Kinect power cord!";
                    break;
                case KinectStatus.Initializing:
                    txtStatusMessage.Text = "Initializing the kinect, please wait.\n (If this isn't done in 20 seconds, try pulling out the Kinect and plugging it back in.)";
                    break;
                default:
                    txtStatusMessage.Text = newKinectStatus.ToString();
                    break;
            }
        }

        /// <summary>
        /// As long as the kinect sees a skeleton, this event gets called
        /// </summary>
        /// <param name="skeleton">Detected skeleton of person in front of kinect</param>
        /// <param name="colorImageFrame">Color image seen by kinect</param>
        /// <param name="depthFrame">Depth image seen by kinect</param>
        void SkeletonAndImageFramesReady(Skeleton skeleton, ColorImageFrame colorImageFrame, DepthImageFrame depthFrame)
        {
            //Check if there's still an image
            

            if (colorImageFrame != null)
            {
                //Has the format changed?               
                bool haveNewFormat = _lastImageFormat != colorImageFrame.Format;
                if (haveNewFormat)
                {
                    _pixelData = new byte[colorImageFrame.PixelDataLength];
                }
                colorImageFrame.CopyPixelDataTo(_pixelData);
                if (haveNewFormat)
                {
                    border = new Border();
                    border.BorderBrush = Brushes.Black;
                    border.BorderThickness = new Thickness(2);
                    _kinectColorImage.Visibility = Visibility.Visible;
                    _outputImage = new WriteableBitmap(
                        colorImageFrame.Width,
                        colorImageFrame.Height,
                        96,  // DpiX
                        96,  // DpiY
                        PixelFormats.Bgr32,
                        null);
                    //Update our last detected image for a player picture.
                    LastDetectedImage = _outputImage;
                    _kinectColorImage.Source = _outputImage;
                    border.Width = _kinectColorImage.Width;
                    border.Height = _kinectColorImage.Height;
                }
                _outputImage.WritePixels(
                        new Int32Rect(0, 0, colorImageFrame.Width, colorImageFrame.Height),
                        _pixelData,
                        colorImageFrame.Width * Bgr32BytesPerPixel,
                        0);
                _lastImageFormat = colorImageFrame.Format;
            }
            ImageContainer.Children.Clear();
            ImageContainer.Children.Add(_kinectColorImage);
            ImageContainer.Children.Add(border);
            if (depthFrame != null && skeleton != null)
            {
                GetCameraPoint(skeleton, depthFrame);
            }
        }

        /// <summary>
        /// Map joints to the color image
        /// </summary>
        /// <param name="skeleton">Skeleton we want to map</param>
        /// <param name="depthFrame">depthframe we create the point we want to map from</param>
        void GetCameraPoint(Skeleton skeleton, DepthImageFrame depthFrame)
        {
            Joint[] joints = (from j in skeleton.Joints
                              where j.TrackingState == JointTrackingState.Tracked
                              select j).ToArray();

            //Creating color image points for every joint in the skeleton so we can draw them
            foreach (ColorImagePoint colorImagePoint in joints.Select(joint => depthFrame.MapFromSkeletonPoint(joint.Position)).Select(depthImagePoint => 
                    depthFrame.MapToColorImagePoint(depthImagePoint.X, depthImagePoint.Y, ColorImageFormat.RgbResolution640x480Fps30)))
            {
                CameraPosition(new Ellipse(), colorImagePoint);
            }
        }

        /// <summary>
        /// Add the joints to the color image
        /// </summary>
        /// <param name="el">Ellipse to draw</param>
        /// <param name="point">Position we will draw the ellipse on the colorviewer.</param>
        private void CameraPosition(Ellipse el, ColorImagePoint point)
        {
            el.Fill = Brushes.Red;
            el.StrokeThickness = 1;
            el.Width = 8;
            el.Height = 8;
            el.Stroke = Brushes.White;
            //Divide by 2 for width and height so point is right in the middle 
            //instead of in top/left corner
            Canvas.SetLeft(el, (point.X - el.Width) / 2);
            Canvas.SetTop(el, (point.Y - el.Height) / 2);
            if (!(point.X / 2 > 320 || point.Y / 2 > 240))
            {
                ImageContainer.Children.Add(el);
            }
        }

    }
}

